#include "ti_msp_dl_config.h"
#include "control.h"

#include "pid.h"
#include "stdio.h"
#include "math.h"



void motor_set_pwm(float pid_out,float pid_out1)  //100%PWM   97.2°/ms
{
	float x = pid_out,x1 = pid_out1;
	uint16_t y,y1;
	
	if(x > 0 )
	{
		motor_forward();
	}
	else {
		motor_back();
	}
	x  = pid_out / 97.2f * 2000.0f;
	x1 = pid_out1 / 97.2f * 2000.0f;
	
	y  = fabs(x);
	y1 = fabs(x1);
	
    if(y > 1999 )
    {
        y = 1999;
    }
	DL_TimerG_setCaptureCompareValue(PWM_0_INST,1999-y,DL_TIMER_CC_0_INDEX);  // 400即为20%占空比（Period=1999）
	
}


void motor_speed_pid(void)
{
	speed_pid_realize(&pid_speed);
	motor_set_pwm(pid_speed.setout_val, 0);
}

void motor_location_pid(int16_t xcm)
{
	float y;
	pid_location.target_val = xcm * 1586.7;    //设置目标值为需要转的总角度
	location_pid_realize_nodie(&pid_location);
	y = pid_location.setout_val / pid_location.target_val *80;   //动态调整，不要一直除285606（1586.7不准）
	motor_set_pwm(y, 0);
}

void motor_angle_pid(float hope_angle)
{
	float y;
	pid_angle.target_val = hope_angle;
	angle_pid_realize(&pid_angle);
	y = pid_angle.setout_val;
	if( y > 30)
	{
		y = 30;
	}
	else if(y < -30){
		y = -30;
	}
	motor_set_pwm(pid_speed.actual_val-pid_angle.setout_val, pid_speed2.actual_val + pid_angle.setout_val);
}


void motor_location_and_speed(int16_t xcm)
{
	float y;
	pid_location.target_val = xcm * 1586.7;    //设置目标值为需要转的总角度
	location_pid_realize(&pid_location);
	pid_speed.target_val = pid_location.setout_val / pid_location.target_val *80;   //动态调整，不要一直除285606（1586.7不准）
	speed_pid_realize(&pid_speed);
	y = pid_speed.setout_val;
	motor_set_pwm(y, 0);
}

void motor_forward(void)   //一个轮子前进模式
{
	// 将AIN1引脚设置为高电平
  DL_GPIO_setPins(Motor_AIN1_PORT,Motor_AIN1_PIN);
		// 将AIN2引脚设置为低电平
  DL_GPIO_clearPins(Motor_AIN2_PORT,Motor_AIN2_PIN);
}

void motor_back(void)    //一个轮子后退模式
{
	// 将AIN2引脚设置为高电平
  DL_GPIO_setPins(Motor_AIN2_PORT,Motor_AIN2_PIN);
		// 将AIN1引脚设置为低电平
  DL_GPIO_clearPins(Motor_AIN1_PORT,Motor_AIN1_PIN);
}

void motor_close(void)   //强行制动模式
{
			// 将AIN1引脚设置为高电平
    DL_GPIO_setPins(Motor_AIN1_PORT,Motor_AIN1_PIN);
		// 将AIN2引脚设置为高电平
    DL_GPIO_setPins(Motor_AIN2_PORT,Motor_AIN2_PIN);
}





